#include "CustomFrameListener.h"

//DEFAULT CTOR
CustomFrameListener::CustomFrameListener()
{
	//RESET GAME TIME
	time = 0.0;
}
//CTOR
CustomFrameListener::CustomFrameListener(Ogre::SceneManager* iSceneManager, Ogre::SceneNode* node, Ogre::Entity* ent, RenderWindow* win, Ogre::Camera* cam, xn::Context context, XnVSessionManager* sessionManager, xn::UserGenerator UserGenerator, XnUserID candidateID)
{
	//RESET GAME TIME
	time = 0.0;

	//MEMBER PRIME_SENSE
	m_UserGenerator = UserGenerator;
	m_candidateID = candidateID;
	m_context = context;
	m_sessionManager = sessionManager;

	//MEMBER SCENE
	_Cam = cam;
	_node = node;
	_ent = ent;
	mSceneManager = iSceneManager;

	//MEMBER MOVEMENT
	_WalkingSpeed = 50.0f;
	_rotation = 0.0f;

	//INPUT MANAGER
	size_t windowHnd = 0;
	std::stringstream windowHndStr;
	win->getCustomAttribute("WINDOW", &windowHnd);
	windowHndStr << windowHnd;
	OIS::ParamList pl;
	pl.insert(std::make_pair(std::string("WINDOW"), windowHndStr.str()));
	_man = OIS::InputManager::createInputSystem( pl );

	//INPUT MEMBER
	_key = static_cast<OIS::Keyboard*>(_man->createInputObject(
		OIS::OISKeyboard, false ));
	_mouse = static_cast<OIS::Mouse*>(_man->createInputObject(
		OIS::OISMouse, false ));
}

//DTOR
CustomFrameListener::~CustomFrameListener()
{
	_man->destroyInputObject(_key);
	_man->destroyInputObject(_mouse);
	OIS::InputManager::destroyInputSystem(_man);
}

bool CustomFrameListener::frameStarted(const Ogre::FrameEvent &evt)
{
	//UPDATE context & sessionManager PRIME_SENSE
	m_context.WaitNoneUpdateAll();
	m_sessionManager->Update(&m_context);

	//UPDATE SKELETON CHARACTER
	updateSkeleton();

	//TRANSLATE CAMERA
	Ogre::Vector3 translate(0,0,0);

	//CAPTURE INPUT
	_key->capture();
	_mouse->capture();

	//QUIT WINDOW
	if(_key->isKeyDown(OIS::KC_ESCAPE))
	{
		return false;
	}

	//MOVEMENT CAMERA
	if(_key->isKeyDown(OIS::KC_W))
	{
		translate += Ogre::Vector3(0,0,-1);
		_rotation = 3.14f;
	}
	if(_key->isKeyDown(OIS::KC_S))
	{
		translate += Ogre::Vector3(0,0,1);
		_rotation = 0.0f;
	}
	if(_key->isKeyDown(OIS::KC_A))
	{
		translate += Ogre::Vector3(-1,0,0);
		_rotation = -1.57f;
	}
	if(_key->isKeyDown(OIS::KC_D))
	{
		translate += Ogre::Vector3(1,0,0);
		_rotation = 1.57f;
	}

	//ROTATION CAMERA
	float rotX = _mouse->getMouseState().X.rel * evt.timeSinceLastFrame* -1;
	float rotY = _mouse->getMouseState().Y.rel * evt.timeSinceLastFrame * -1;
	_Cam->yaw(Ogre::Radian(rotX));
	_Cam->pitch(Ogre::Radian(rotY));
	_Cam->moveRelative((translate*100)*evt.timeSinceLastFrame);

	if(m_candidateID != 0)
	{
#if DEBUG
		///////////////////////////////////////////////////////////////////////////////
		//			ONLY FOR DEBUGGING THE EFFECTIVE JOINTS' POSITION
		////////////////////////////////////////////////////////////////////////////////
		
		////////////////////////////////////////////////////////////////////////////////
		// Creates a new sphere with an ID (make sure node's ID matches with sphere's ID
		// ball = new GameSphere(..., "Ball-ID", ...);
		// entity = (..., "Ball-ID");
		////////////////////////////////////////////////////////////////////////////////

		//static GameSphere* ball = new GameSphere(0.0f, Vector3(0,0,1), "Ball-ID", 1, 16, 16);
		//static bool first = true;
		//if(first)
		//{
		//	first = false;

		//	Entity* ent = mSceneManager->createEntity("BallEntity", "Ball-ID");
		//	SceneNode* node = mSceneManager->getRootSceneNode()->createChildSceneNode();
		//	node->attachObject(ent);

		//	ball->SetSphereEntity(ent);
		//	ball->SetSphereNode(node);
		//	ball->GetSphereEntity()->setMaterialName("Examples/Rockwall");

		//	balls.push_back(ball);
		//}
#endif
		XnSkeletonJointPosition rightHandPos;
		m_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(m_candidateID, XN_SKEL_RIGHT_HAND, rightHandPos);
		
#if DEBUG
		///////////////////////////////////////////////////////////////////////////
		// Set the sphere's position in WHAT_EVER_JOINT
		///////////////////////////////////////////////////////////////////////////

		///////////////////////////////////////////////////////////////////////////
		// Hard-coded positions.
		///////////////////////////////////////////////////////////////////////////
		//if(rightHandPos.fConfidence > 0.5f)
		//{
		//	ball->GetSphereNode()->setPosition(-rightHandPos.position.X/50-5, rightHandPos.position.Y/50, -rightHandPos.position.Z/50 + _node->getPosition().z+50);
		//}
#endif
		updateBalls(evt.timeSinceLastFrame);
		checkCollisions(evt.timeSinceLastFrame);

		time += evt.timeSinceLastFrame;

		if(time > 3.0)
		{

			//Ogre::SceneNode* node = mSceneManager->createSceneNode();
			//mSceneManager->getRootSceneNode()->addChild(node);
			//Ogre::Entity* ballEnt = mSceneManager->createEntity("sphere.mesh");
			//Ogre::SceneNode* ballNode = node->createChildSceneNode();
			//ballNode->setPosition(Ogre::Vector3(0.0f,-10.0f,500.0f));
			//ballNode->setScale(Ogre::Vector3(0.1f,0.1f,0.1f));
			//mSceneManager->getSceneNode(ballNode->getName())->createChildSceneNode()->attachObject(ballEnt);

			//balls.push_back(ballNode);
			//ballsEnt.push_back(ballEnt);
			Ogre::Real posX(rand()%40-20);
			Ogre::Real posY(rand()%10+15);
			GameSphere* ball = new GameSphere(-40.0f, Vector3(0,0,1), "Ball-ID", 10, 16, 16);

			Entity* ent = mSceneManager->createEntity("Ball-ID");
			SceneNode* node = mSceneManager->getRootSceneNode()->createChildSceneNode();
			node->setPosition(posX, posY, 500.0f);
			node->attachObject(ent);

			ball->SetSphereEntity(ent);
			ball->SetSphereNode(node);
			ball->GetSphereEntity()->setMaterialName("Examples/Rockwall");

			balls.push_back(ball);

			time = 0.0;
		}
	}
	return true;
}

void CustomFrameListener::checkCollisions(const Real& i_deltaTime)
{
	XnSkeletonJointPosition rightHandPos;
	m_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(m_candidateID, XN_SKEL_RIGHT_HAND, rightHandPos);

	XnSkeletonJointPosition leftHandPos;
	m_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(m_candidateID, XN_SKEL_LEFT_HAND, leftHandPos);

	std::vector<GameSphere*>::iterator it;
	Ogre::Vector3 transformedPosition;
	Ogre::Real tolerance(25);
	Ogre::Real squaredDistance(0);
	bool collided = false;
	for(it = balls.begin(); it!=balls.end(); ++it)
	{
		collided = false;
		/////////////////////////////////////////////////////////////////
		//					RIGHT HAND COLLISION
		/////////////////////////////////////////////////////////////////
		//Need to manually transform JOINTS positions. Maybe Sinbad's skeleton is in local space and then converted to world space
		transformedPosition.x = -rightHandPos.position.X/50-5;
		transformedPosition.y = rightHandPos.position.Y/50;
		transformedPosition.z = -rightHandPos.position.Z/50 + _node->getPosition().z+50;
		squaredDistance = (*it)->GetSphereNode()->getPosition().squaredDistance(transformedPosition);
		if(squaredDistance <= (*it)->GetRadius()*(*it)->GetRadius() + tolerance)
		{
			(*it)->SetSpeed(+100);
			collided = true;
		}

		/////////////////////////////////////////////////////////////////
		//					LEFT HAND COLLISION
		/////////////////////////////////////////////////////////////////
		if(!collided)
		{

			transformedPosition.x = -leftHandPos.position.X/50+5;
			transformedPosition.y = leftHandPos.position.Y/50;
			transformedPosition.z = -leftHandPos.position.Z/50 + _node->getPosition().z+50;
			squaredDistance = (*it)->GetSphereNode()->getPosition().squaredDistance(transformedPosition);
			if(squaredDistance <= (*it)->GetRadius()*(*it)->GetRadius() + tolerance)
			{
				(*it)->SetSpeed(+100);
				collided = true;
			}
		}
	}
}

//UPDATE SKELETON FUNCTION
void CustomFrameListener::updateSkeleton()
{
	static bool bNewUser = true;
	static bool bRightAfterSwardsPositionChanged = false;
	static Vector3 origTorsoPos;

	mGoalDirection = Vector3::ZERO;   // we will calculate this
	xn::SkeletonCapability pUserSkel = m_UserGenerator.GetSkeletonCap();

	Skeleton* skel = _ent->getSkeleton();
	Ogre::Bone* rootBone = skel->getBone("Root");

	XnSkeletonJointPosition torsoPos;

	if (pUserSkel.IsTracking(m_candidateID))
	{
		if(bNewUser)
		{			
			pUserSkel.GetSkeletonJointPosition(m_candidateID, XN_SKEL_TORSO, torsoPos);
			if(torsoPos.fConfidence > 0.5)
			{
				origTorsoPos.x = -torsoPos.position.X;
				origTorsoPos.y = torsoPos.position.Y;
				origTorsoPos.z = -torsoPos.position.Z;
				bNewUser = false;
			}
		}

		transformBone("Stomach",XN_SKEL_TORSO, true);
		transformBone("Waist", XN_SKEL_WAIST);
		transformBone("Root", XN_SKEL_WAIST);
		transformBone("Chest",XN_SKEL_TORSO, true);
		transformBone("Humerus.L",XN_SKEL_LEFT_SHOULDER);
		transformBone("Humerus.R",XN_SKEL_RIGHT_SHOULDER);
		transformBone("Ulna.L",XN_SKEL_LEFT_ELBOW);
		transformBone("Ulna.R",XN_SKEL_RIGHT_ELBOW);
		transformBone("Thigh.L",XN_SKEL_LEFT_HIP);
		transformBone("Thigh.R",XN_SKEL_RIGHT_HIP);
		transformBone("Calf.L",XN_SKEL_LEFT_KNEE);
		transformBone("Calf.R",XN_SKEL_RIGHT_KNEE);

		if(!bNewUser)
		{			 
			pUserSkel.GetSkeletonJointPosition(m_candidateID, XN_SKEL_TORSO, torsoPos);
			Vector3 newPos;
			newPos.x = -torsoPos.position.X;
			newPos.y = torsoPos.position.Y;
			newPos.z = -torsoPos.position.Z;

			Vector3 newPos2 = (newPos - origTorsoPos)/100;

			newPos2.y -= 0.3;

			if (newPos2.y < 0)
			{
				newPos2.y /= 2.5;

				if (newPos2.y < -1.5)
				{
					newPos2.y = -1.5;
				}
			}
			if(torsoPos.fConfidence > 0.5)
			{
				rootBone->setPosition(newPos2);
			}
		}
	}
}


//BONE TRANSFORM
void CustomFrameListener::transformBone(const Ogre::String& modelBoneName, XnSkeletonJoint skelJoint, bool flip)
{
	// Get the model skeleton bone info
	Skeleton* skel = _ent->getSkeleton();
	Ogre::Bone* bone = skel->getBone(modelBoneName);
	Ogre::Quaternion qI = bone->getInitialOrientation();
	Ogre::Quaternion newQ = Quaternion::IDENTITY;

	// Get the openNI bone info
	xn::SkeletonCapability pUserSkel = m_UserGenerator.GetSkeletonCap();		
	XnSkeletonJointOrientation jointOri;
	pUserSkel.GetSkeletonJointOrientation(m_candidateID, skelJoint, jointOri);

	static float deg = 0;
	if(jointOri.fConfidence > 0 )
	{
		XnVector3D col1 = xnCreatePoint3D(jointOri.orientation.elements[0], jointOri.orientation.elements[3], jointOri.orientation.elements[6]);
		XnVector3D col2 = xnCreatePoint3D(jointOri.orientation.elements[1], jointOri.orientation.elements[4], jointOri.orientation.elements[7]);
		XnVector3D col3 = xnCreatePoint3D(jointOri.orientation.elements[2], jointOri.orientation.elements[5], jointOri.orientation.elements[8]);

		Ogre::Matrix3 matOri(jointOri.orientation.elements[0],-jointOri.orientation.elements[1],jointOri.orientation.elements[2],
			-jointOri.orientation.elements[3],jointOri.orientation.elements[4],-jointOri.orientation.elements[5],
			jointOri.orientation.elements[6],-jointOri.orientation.elements[7],jointOri.orientation.elements[8]);
		Quaternion q;

		newQ.FromRotationMatrix(matOri);

		bone->resetOrientation(); //in order for the conversion from world to local to work.
		newQ = bone->convertWorldToLocalOrientation(newQ);

		bone->setOrientation(newQ*qI);			
	} 
}

void CustomFrameListener::updateBalls(const Ogre::Real& i_deltaTime)
{
	//INTEGRATION PHISYCS (TODO)
	std::vector<GameSphere*>::iterator it;

	for(it = balls.begin(); it != balls.end(); ++it)
	{
		if((*it)->GetSphereNode()->getPosition().z > 600)
		{	
			(*it)->SetSpeed(0);
			(*it)->GetSphereNode()->setPosition(0.f, 0.f, -500.0f);
		}
		else
		{
			(*it)->MoveOnZAxis(i_deltaTime);
		}
	}

	//CHECK COLLISION (TODO)
}
